﻿using System;
using System.Threading;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using Microsoft.Kinect;

namespace WpfApplication1
{
    public class ThreadParameters
    {
        public int x;
        public int y;
        //public bool[] hasAnalyzed;
        public int lastx;
        public int lasty;

        public ThreadParameters(int xx, int yy)
        {
            x = xx;
            y = yy;
            //hasAnalyzed = arr;
            lastx = -1;
            lasty = -1;
        }

        public ThreadParameters(int xx, int yy, int lx, int ly)
        {
            x = xx;
            y = yy;
            lastx = lx;
            lasty = ly;
        }
    }
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class MainWindow : Window
    {
        KinectSensor sensor = KinectSensor.KinectSensors.First();
        private byte[] pixelData;
        private byte[] depthFrame32;
        private short[] pixelDepthData;
        private byte[] filteredData;
        private ColorImagePoint[] cpoints;
        private WriteableBitmap outputImage;
        private WriteableBitmap outDepthImage;
        private ColorImageFormat lastImageFormat = ColorImageFormat.Undefined;
        private DepthImageFormat lastDepthFormat = DepthImageFormat.Undefined;
        private ColorImageFormat currentCFormat = ColorImageFormat.Undefined;

        private const int RedIndex = 2;
        private const int GreenIndex = 1;
        private const int BlueIndex = 0;
        private int width;
        private int height;
        private SkeletonPoint head;
        private SkeletonPoint leftHand;
        private SkeletonPoint rightHand;
        private int frameCount = 0;//allows skipping of frames when needed to improve performance.

        private static readonly int[] IntensityShiftByPlayerR = { 1, 2, 0, 2, 0, 0, 2, 0 };
        private static readonly int[] IntensityShiftByPlayerG = { 1, 2, 2, 0, 2, 0, 0, 1 };
        private static readonly int[] IntensityShiftByPlayerB = { 1, 0, 2, 2, 0, 2, 0, 2 };

        private static readonly int Bgr32BytesPerPixel = (PixelFormats.Bgr32.BitsPerPixel + 7) / 8;

        //blob tracking
        /*private static BlobTrackerAuto<Bgr> _tracker;
        private static IBGFGDetector<Bgr> _detector;*/

        public MainWindow()
        {
            InitializeComponent();

           /* _detector = new FGDetector<Bgr>(Emgu.CV.CvEnum.FORGROUND_DETECTOR_TYPE.FGD);
            _tracker = new BlobTrackerAuto<Bgr>();*/
            head.X = -1;
            head.Y = -1;
            head.Z = 1000;
            leftHand.Z = 1000;
            rightHand.Z = 1000;


            sensor.DepthStream.Range = DepthRange.Near;
            sensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
            sensor.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);
            sensor.Start();

            sensor.AllFramesReady += this.AllImageReady;
            
        }

        ~MainWindow()
        {
            sensor.Stop();
        }


        void AllImageReady(object sender, AllFramesReadyEventArgs e)
        {
            //imageText.Text = "was";
            using (ColorImageFrame cFrame = e.OpenColorImageFrame())
            using (DepthImageFrame imageFrame = e.OpenDepthImageFrame())
            {
                
                if (imageFrame != null && cFrame != null)
                {
                    //and frame tick by 1
                    frameCount++;
                    if (frameCount > 999)//make sure int doesn't go out-of-bounds
                    {
                        frameCount = 0;
                    }
                    //sets width and height values
                    width = cFrame.Width;
                    height = cFrame.Height;

                    //set current color Format
                    currentCFormat = cFrame.Format;

                    //process color frame
                    processColorImage(sender, cFrame);
                    filteredData = (byte[])pixelData.Clone();

                    //process depth frame
                    processDepthImage(sender, imageFrame);

                    //output filtered color image
                    this.outputImage.WritePixels(
                    new Int32Rect(0, 0, width, height),
                    this.pixelData,
                    cFrame.Width * Bgr32BytesPerPixel,
                    0);
                    
                    this.lastImageFormat = cFrame.Format;
                    this.lastDepthFormat = imageFrame.Format;
                    //sensor.AllFramesReady -= this.AllImageReady;
                }
            }
        }


        //processes depthFrame, and return a array of pixelcolors based on depth
        private byte[] processDepthImage(object sender, DepthImageFrame imageFrame)
        {
            /**DEPTH PROCESSING**/
            // We need to detect if the format has changed.
            bool haveNewFormat = this.lastDepthFormat != imageFrame.Format;

            if (haveNewFormat)
            {
                this.pixelDepthData = new short[imageFrame.PixelDataLength];
                this.depthFrame32 = new byte[width * height * Bgr32BytesPerPixel];
            }

            imageFrame.CopyPixelDataTo(this.pixelDepthData);

            //sycronize depth and color frames
            //cpoints = new ColorImagePoint[imageFrame.Height*imageFrame.Width];
            //sensor.MapDepthFrameToColorFrame(imageFrame.Format, pixelDepthData, this.currentCFormat, cpoints);

            byte[] convertedDepthBits = this.ConvertDepthFrame(this.pixelDepthData, ((KinectSensor)sender).DepthStream);

            //sample color of specific pixel
            int y = 260;
            int x = 320;
            imageText.Text = convertedDepthBits[(640 * y + x) * 4] + ", " + convertedDepthBits[((640 * y + x) * 4) + 1] + ", " + convertedDepthBits[((640 * y + x) * 4) + 2];
            //turn pixel and surroundings white to identifiy sampled pixel
            for (int i = -5; i < 6; i++)
            {
                for (int j = -5; j < 6; j++)
                {
                    convertedDepthBits[(640 * (y + i) + x + j) * 4] = 255; //to set iterator to specific row it's rowWidth*row, then add offset so rowWidth*row+column
                    convertedDepthBits[(640 * (y + i) + x + j) * 4 + 1] = 255;
                    convertedDepthBits[(640 * (y + i) + x + j) * 4 + 2] = 255;
                }
            }

            //if object is offscreen only serch for it every third frame
            if (head.Z >= 120/* && frameCount % 3 == 0*/)//head
            {
                trackHead(convertedDepthBits);
            }
            else if(head.Z<120)
            {
                updateObject(convertedDepthBits, 0);
            }
            if (leftHand.Z >= 120 && frameCount % 3 == 0 && head.Z<1000)//left hand
            {
                trackHand(convertedDepthBits, 0);
            }
            else if(head.Z<120 && leftHand.Z<120)
            {
                updateObject(convertedDepthBits, 2);
            }
            if (rightHand.Z >= 120 && frameCount % 3 == 0 && head.Z<1000)//right hand
            {
                trackHand(convertedDepthBits, 1);
            }
            else if(head.Z<120 && rightHand.Z<120)
            {
                updateObject(convertedDepthBits, 1);
            }


            // A WriteableBitmap is a WPF construct that enables resetting the Bits of the image.
            // This is more efficient than creating a new Bitmap every frame.
            if (haveNewFormat)
            {
                depthImage.Visibility = Visibility.Visible;
                this.outDepthImage = new WriteableBitmap(
                    width,
                    height,
                    96,  // DpiX
                    96,  // DpiY
                    PixelFormats.Bgr32,
                    null);
                this.depthImage.Source = this.outDepthImage;
            }

            this.outDepthImage.WritePixels(
                new Int32Rect(0, 0, width, height),
                convertedDepthBits,
                width * Bgr32BytesPerPixel,
                0);
            return convertedDepthBits;
        }

        //process imageFrame
        private void processColorImage(object sender, ColorImageFrame cFrame)
        {
            /**COLOR PROCESSING**/
            // We need to detect if the format has changed.
            bool haveNewFormat = this.lastImageFormat != cFrame.Format;

            if (haveNewFormat)
            {
                this.pixelData = new byte[cFrame.PixelDataLength];
            }

            cFrame.CopyPixelDataTo(this.pixelData);
            //sample color of specific pixel
            int cY = 260;
            int cX = 320;
            clickText.Text = pixelData[(640 * cY + cX) * 4] + ", " + pixelData[((640 * cY + cX) * 4) + 1] + ", " + pixelData[((640 * cY + cX) * 4) + 2];
            //turn pixel and surroundings white to identifiy sampled pixel
            for (int i = -5; i < 6; i++)
            {
                for (int j = -5; j < 6; j++)
                {
                    pixelData[(640 * (cY + i) + cX + j) * 4] = 255; //to set iterator to specific row it's rowWidth*row, then add offset so rowWidth*row+column
                    pixelData[(640 * (cY + i) + cX + j) * 4 + 1] = 255;
                    pixelData[(640 * (cY + i) + cX + j) * 4 + 2] = 255;
                }
            }

            // A WriteableBitmap is a WPF construct that enables resetting the Bits of the image.
            // This is more efficient than creating a new Bitmap every frame.
            if (haveNewFormat)
            {
                colorImage.Visibility = Visibility.Visible;
                this.outputImage = new WriteableBitmap(
                    cFrame.Width,
                    cFrame.Height,
                    96,  // DpiX
                    96,  // DpiY
                    PixelFormats.Bgr32,
                    null);
                this.colorImage.Source = this.outputImage;
            }

            
        }

        // Converts a 16-bit grayscale depth frame which includes player indexes into a 32-bit frame
        // that displays different players in different colors
        private byte[] ConvertDepthFrame(short[] depthFrame, DepthImageStream depthStream)
        {
            int inRange = 11;
            int tooNearDepth = depthStream.TooNearDepth;
            int tooFarDepth = depthStream.TooFarDepth;
            int unknownDepth = depthStream.UnknownDepth;

            //average finder
           /* int xav = 0;
            int yav = 0;
            long totalSamp = 0;*/

            for (int i16 = 0, i32 = 0; i16 < depthFrame.Length && i32 < this.depthFrame32.Length; i16++, i32 += 4)
            {
                int player = depthFrame[i16] & DepthImageFrame.PlayerIndexBitmask;
                int realDepth = depthFrame[i16] >> DepthImageFrame.PlayerIndexBitmaskWidth;

                // transform 13-bit depth information into an 8-bit intensity appropriate
                // for display (we disregard information in most significant bit)
                byte intensity = (byte)(~(realDepth >> 4));

                //find position in color array
                //int cpos = (cpoints[i16].Y * width + cpoints[i16].X) * 4;
                this.filteredData[i32 + RedIndex] = 0;
                this.filteredData[i32 + GreenIndex] = 255;
                this.filteredData[i32 + BlueIndex] = 0;

                if (player == 0 && realDepth == 0)
                {
                    inRange = 0;
                    // white 
                    this.depthFrame32[i32 + RedIndex] = 255;
                    this.depthFrame32[i32 + GreenIndex] = 255;
                    this.depthFrame32[i32 + BlueIndex] = 255;

                    //change all irrelevant pixels to black
                   /*this.filteredData[i32 + RedIndex] = 0;
                    this.filteredData[i32 + GreenIndex] = 0;
                    this.filteredData[i32 + BlueIndex] = 0;*/ 
                }
                else if (player == 0 && realDepth == tooFarDepth)
                {
                    inRange = 0;
                    // dark purple
                    this.depthFrame32[i32 + RedIndex] = 66;
                    this.depthFrame32[i32 + GreenIndex] = 0;
                    this.depthFrame32[i32 + BlueIndex] = 66;

                    //change all irrelevant pixels to black
                    /*this.filteredData[i32 + RedIndex] = 0;
                    this.filteredData[i32 + GreenIndex] = 0;
                    this.filteredData[i32 + BlueIndex] = 0; */
                }
                else if (player == 0 && realDepth == unknownDepth)
                {
                    // dark brown
                    this.depthFrame32[i32 + RedIndex] = 66;
                    this.depthFrame32[i32 + GreenIndex] = 66;
                    this.depthFrame32[i32 + BlueIndex] = 33;

                    //change all irrelevant pixels to black 
                    //if previous pixel was in proper range, treat the next 10 pixels as if they're inrange for the color filter
                    if (inRange == 0)
                    {
                        /*this.filteredData[i32 + RedIndex] = 0;
                        this.filteredData[i32 + GreenIndex] = 0;
                        this.filteredData[i32 + BlueIndex] = 0;*/
                    }
                    else
                    {
                        inRange++;
                        if (inRange > 20)
                        {
                            inRange = 0;
                        }
                    }
                }
                else
                {
                    inRange=1;
                    // tint the intensity by dividing by per-player values
                    this.depthFrame32[i32 + RedIndex] = (byte)(intensity >> IntensityShiftByPlayerR[player]);
                    this.depthFrame32[i32 + GreenIndex] = (byte)(intensity >> IntensityShiftByPlayerG[player]);
                    this.depthFrame32[i32 + BlueIndex] = (byte)(intensity >> IntensityShiftByPlayerB[player]);
                    /*this.pixelData[i32 + RedIndex] = 255;
                    this.pixelData[i32 + GreenIndex] = 255;
                    this.pixelData[i32 + BlueIndex] = 255;*/
                    //record average conglomerate of in range pixels
                    /*xav += (i16) % width;
                    yav += (i16) / width;
                    totalSamp++;*/
                    
                }
            }
            /*ThreadParameters tp;
            //Thread th = new Thread(this.filterPixels);
            if (totalSamp == 0)
            {
                tp = new ThreadParameters((int)(width / 2), (int)(height / 2));
            }
            else
            {
                tp = new ThreadParameters((int)(xav / totalSamp), (int)(yav / totalSamp));
            }
            this.filterPixels(tp);
            //threadNum++;
            //th.Start(tp);
            //this.filterPixels((int)(xav / totalSamp), (int)(yav / totalSamp), new bool[width*height],-1,-1); 
            

            //turn pixel and surroundings white to identifiy sampled pixel
            for (int i = -5; i < 6; i++)
            {
                for (int j = -5; j < 6; j++)
                {
                    if (totalSamp > 0)
                    {
                        pixelData[(640 * (yav / totalSamp + i) + xav / totalSamp + j) * 4] = 0; //to set iterator to specific row it's rowWidth*row, then add offset so rowWidth*row+column
                        pixelData[(640 * (yav / totalSamp + i) + xav / totalSamp + j) * 4 + 1] = 0;
                        pixelData[(640 * (yav / totalSamp + i) + xav / totalSamp + j) * 4 + 2] = 255;
                        depthFrame32[(640 * (yav / totalSamp + i) + xav / totalSamp + j) * 4] = 0; //to set iterator to specific row it's rowWidth*row, then add offset so rowWidth*row+column
                        depthFrame32[(640 * (yav / totalSamp + i) + xav / totalSamp + j) * 4 + 1] = 0;
                        depthFrame32[(640 * (yav / totalSamp + i) + xav / totalSamp + j) * 4 + 2] = 255;
                    }
                }
            }*/

            return this.depthFrame32;
        }

        /*private void filterPixels(ThreadParameters arg){
            Queue<ThreadParameters> q = new Queue<ThreadParameters>();
            q.Enqueue(arg);
            bool[] hasAnalyzed = new bool[width*height];
            hasAnalyzed[width * (arg.y) + (arg.x)] = true;
            ThreadParameters tp;
            while (q.Count() > 0)
            {
                tp = q.Dequeue();
                //process queue
                int index = width * tp.y + tp.x;
                int lastInd = width * tp.lasty + tp.lastx;

                //if it isn't beginning analyze
                if (tp.lastx != -1)
                {
                    int net = pixelData[index * 4 + RedIndex] + pixelData[index * 4 + GreenIndex] + pixelData[index * 4 + BlueIndex];
                    net = net - (pixelData[lastInd * 4 + RedIndex] + pixelData[lastInd * 4 + GreenIndex] + pixelData[lastInd * 4 + BlueIndex]);
                    //Console.WriteLine(net);
                    
                    if (net < 0)
                        net = net * -1;
                    //if the difference between the two pixels is not significant do not filter it.
                    if (net < 100 && depthFrame32[index*4 + GreenIndex] != 0)
                    {
                        filteredData[(index) * 4 + RedIndex] = pixelData[index * 4 + RedIndex];
                        filteredData[(index) * 4 + GreenIndex] = pixelData[index * 4 + GreenIndex];
                        filteredData[(index) * 4 + BlueIndex] = pixelData[index * 4 + BlueIndex];
                    }
                    else
                    {
                        continue;
                    }
                }
                else
                {
                    filteredData[(index) * 4 + RedIndex] = pixelData[index * 4 + RedIndex];
                    filteredData[(index) * 4 + RedIndex] = pixelData[index * 4 + GreenIndex];
                    filteredData[(index) * 4 + RedIndex] = pixelData[index * 4 + BlueIndex];
                }

                //insert into queue
                //call surrounding pixels if they have not already been visited
                if ((tp.x - 1 > -1 && tp.x + 1 < width) && (tp.y - 1 > -1 && tp.y + 1 < height))
                {
                    if (tp.y >= 479) { 
                        Console.WriteLine("inside: tp.y = " + tp.y); }
                    if (hasAnalyzed[width * (tp.y - 1) + (tp.x)]==false)
                    {
                        q.Enqueue(new ThreadParameters(tp.x, tp.y-1, tp.x, tp.y));
                        hasAnalyzed[width * (tp.y - 1) + (tp.x)] = true;
                    }
                    if (hasAnalyzed[width * (tp.y + 1) + (tp.x)] == false)
                    {
                        q.Enqueue(new ThreadParameters(tp.x, tp.y+1, tp.x, tp.y));
                        hasAnalyzed[width * (tp.y + 1) + (tp.x)] = true;
                    }
                    if (hasAnalyzed[width * (tp.y) + (tp.x - 1)] == false)
                    {
                        q.Enqueue(new ThreadParameters(tp.x-1, tp.y, tp.x, tp.y));
                        hasAnalyzed[width * (tp.y) + (tp.x - 1)] = true;
                    }
                    if (hasAnalyzed[width * (tp.y) + (tp.x + 1)] == false)
                    {
                        q.Enqueue(new ThreadParameters(tp.x+1, tp.y, tp.x, tp.y));
                        hasAnalyzed[width * (tp.y) + (tp.x + 1)] = true;
                    }
                    //if (hasAnalyzed[width * (tp.y - 1) + (tp.x - 1)] == false)
                    //{
                    //    q.Enqueue(new ThreadParameters(tp.x - 1, tp.y - 1, tp.x, tp.y));
                    //    hasAnalyzed[width * (tp.y - 1) + (tp.x - 1)] = true;
                    //}
                    //if (hasAnalyzed[width * (tp.y + 1) + (tp.x + 1)] == false)
                    //{
                    //    q.Enqueue(new ThreadParameters(tp.x + 1, tp.y + 1, tp.x, tp.y));
                    //    hasAnalyzed[width * (tp.y + 1) + (tp.x + 1)] = true;
                    //}
                    //if (hasAnalyzed[width * (tp.y - 1) + (tp.x + 1)] == false)
                    //{
                    //    q.Enqueue(new ThreadParameters(tp.x + 1, tp.y - 1, tp.x, tp.y));
                    //    hasAnalyzed[width * (tp.y - 1) + (tp.x + 1)] = true;
                    //}
                    //if (hasAnalyzed[width * (tp.y + 1) + (tp.x - 1)] == false)
                    //{
                    //    q.Enqueue(new ThreadParameters(tp.x - 1, tp.y + 1, tp.x, tp.y));
                    //    hasAnalyzed[width * (tp.y + 1) + (tp.x - 1)] = true;
                    //}
                }

            }

        }*/
        //find outline (using adjusted color fill algorithm)
       /* private void filterPixels(object arg)
        {
            ThreadParameters tp = (ThreadParameters)arg;
            int index = width*tp.y+tp.x;
            int lastInd = width*tp.lasty+tp.lastx;

            //if it isn't beginning analyze
            if (tp.lastx != -1)
            {
                int net = pixelData[index * 4 + RedIndex] + pixelData[index * 4 + GreenIndex] + pixelData[index * 4 + BlueIndex];
                net -= pixelData[lastInd * 4 + RedIndex] + pixelData[lastInd * 4 + GreenIndex] + pixelData[lastInd * 4 + BlueIndex];
                if (net < 0)
                    net = net * -1;
                //if the difference between the two pixels is not significant do not filter it.
                if (net < 150)
                {
                    filteredData[(index) * 4 + RedIndex] = pixelData[index * 4 + RedIndex];
                    filteredData[(index) * 4 + RedIndex] = pixelData[index * 4 + GreenIndex];
                    filteredData[(index) * 4 + RedIndex] = pixelData[index * 4 + BlueIndex];
                }
                else
                {
                    return;
                }
            }
            else
            {
                filteredData[(index) * 4 + RedIndex] = pixelData[index * 4 + RedIndex];
                filteredData[(index) * 4 + RedIndex] = pixelData[index * 4 + GreenIndex];
                filteredData[(index) * 4 + RedIndex] = pixelData[index * 4 + BlueIndex];
            }
            tp.hasAnalyzed[width * tp.y + tp.x] = true;

            //call surrounding pixels if they have not already been visited
            if ((tp.x - 1 > -1 && tp.x + 1 < width) && (tp.y - 1 > -1 && tp.y + 1 < height))
            {
                ThreadParameters pass;
                if (!tp.hasAnalyzed[width * (tp.y - 1) + (tp.x)])
                {
                    pass = new ThreadParameters(tp.x, tp.y - 1, tp.hasAnalyzed);
                    //filterPixels(x, y - 1, hasAnalyzed, tp.x, tp.y);
                }
                if (!tp.hasAnalyzed[width * (tp.y + 1) + (tp.x)])
                {
                    pass = new ThreadParameters(tp.x, tp.y + 1, tp.hasAnalyzed);
                    //filterPixels(x, y + 1, hasAnalyzed, x, y);
                }
                /*if (!hasAnalyzed[width * (y - 1) + (x + 1)])
                {
                    filterPixels(x + 1, y - 1, hasAnalyzed, x, y);
                }
                if (!hasAnalyzed[width * (y + 1) + (x + 1)])
                {
                    filterPixels(x + 1, y + 1, hasAnalyzed, x, y);
                }
                if (!hasAnalyzed[width * (y - 1) + (x - 1)])
                {
                    filterPixels(x - 1, y - 1, hasAnalyzed, x, y);
                }
                if (!hasAnalyzed[width * (y + 1) + (x - 1)])
                {
                    filterPixels(x - 1, y + 1, hasAnalyzed, x, y);
                }
                if (!tp.hasAnalyzed[width * (tp.y) + (tp.x + 1)])
                {
                    pass = new ThreadParameters(tp.x+1 , tp.y, tp.hasAnalyzed);
                    //filterPixels(x + 1, y, hasAnalyzed, x, y);
                }
                if (!tp.hasAnalyzed[width * (tp.y) + (tp.x - 1)])
                {
                    pass = new ThreadParameters(tp.x - 1, tp.y, tp.hasAnalyzed);
                    //filterPixels(x - 1, y, hasAnalyzed, x, y);
                }
            }
                threadNum--;
        }*/

        private byte[] trackHead(byte[] stuff)
        {
            //top left is 0,0 so headMin will be > head Max
            int headMax = 0;
            int headMin = 0;
            int headleft = width;
            int headright = 0;
            int nochange = 0;
            for (int y = 0; y<height; y++)
            {
                for (int x = 0; x < width; x++)
                {
                    //if processing in range data
                    if (stuff[(width * y + x) * 4] > 90 && stuff[(width * y + x) * 4] <= head.Z+1)
                    {
                        if (headMax == 0)
                        {
                            //assume first in range item found is the top of the head
                            headMax = y;
                            headleft = x;
                            headright = x;
                        }
                    }
                }
                if (headMax > 0)
                {
                    //find left head side
                    int x = headleft;
                    while (x > 0 && stuff[(width * y + x - 1) * 4] > 90 && stuff[(width * y + x - 1) * 4] < 110)
                    {
                        x--;
                        if (stuff[(width * y + x - 1) * 4] == 33)//detect unknown distance as noise
                        {
                            int noise = 0;
                            int curx = x;
                            //attempt to skip past noise
                            while (!(stuff[(width * y + x - 1) * 4] > 90 && stuff[(width * y + x - 1) * 4] < 110))
                            {
                                noise++;
                                x--;
                                if (x <= 0)
                                {
                                    break;
                                }
                            }
                            //Console.WriteLine(noise);
                            if (noise > 50 || x <= 0)//if noise value is too big assume it's not noise and detect edge
                            {
                                x = curx;
                                break;
                            }
                        }
                    }
                        
                    //find right head side
                    int xr = headright;
                    while (xr<width-1&&stuff[(width * y + xr + 1) * 4] > 90 && stuff[(width * y + xr + 1) * 4] < 110)
                    {
                        xr++;
                        if (stuff[(width * y + xr - 1) * 4] == 33)//detect unknown distance as noise
                        {
                            int noise = 0;
                            int curx = x;
                            //attempt to skip past noise
                            while (!(stuff[(width * y + xr + 1) * 4] > 90 && stuff[(width * y + xr + 1) * 4] < 110))
                            {
                                noise++;
                                xr++;
                                if (xr >= width-1)
                                {
                                    break;
                                }
                            }
                            //Console.WriteLine(noise);
                            if (noise > 50 || xr >= width-1)//if noise value is too big assume it's not noise and detect edge
                            {
                                xr = curx;
                                break;
                            }
                        }
                    }
                    //assign left and right values
                    if (headleft == x)
                    {
                        nochange++;
                    }
                    else
                    {
                        nochange = 0;
                        headMin = y;
                    }
                    headright = xr;
                    headleft = x;
                    
                }

                //terminate statement: if headleft does not change for 50 y values
                if (nochange > 50 && headMin-headMax>20)
                {
                    break;
                }
            }

            if (headMin == 0)
            {
                headMin = headMax;
            }
            else
            {
                headMin = headMin + (headMin - headMax);
            }

            /*drawDot(width / 2, headMax, stuff, -1);
            drawDot(width / 2, headMin, stuff, -1);
            drawDot(headleft, height / 2, stuff, -1);
            drawDot(headright, height / 2, stuff, -1);*/
            //draw bounding box
            if (headMax < 0)
            {
                headMax = 0;
            }
            if(headMin>height-1){
                headMin = height - 1;
            }
            if(headleft<0){
                headleft = 0;
            }
            if(headright>width-1){
                headright = width-1;
            }
            if (headMin > 0)
            {
                for (int i = 0; i < 2; i++)
                {
                    for (int x = headleft; x <= headright; x++)
                    {
                        stuff[(width * headMin + x - i) * 4 + RedIndex] = 255;
                        stuff[(width * headMin + x - i) * 4 + GreenIndex] = 0;
                        stuff[(width * headMin + x - i) * 4 + BlueIndex] = 0;
                        stuff[(width * headMax + x - i) * 4 + RedIndex] = 255;
                        stuff[(width * headMax + x - i) * 4 + GreenIndex] = 0;
                        stuff[(width * headMax + x - i) * 4 + BlueIndex] = 0;
                    }
                    for (int y = headMin; y >= headMax; y--)
                    {
                        stuff[(width * (y - i) + headleft) * 4 + RedIndex] = 255;
                        stuff[(width * (y - i) + headleft) * 4 + GreenIndex] = 0;
                        stuff[(width * (y - i) + headleft) * 4 + BlueIndex] = 0;
                        stuff[(width * (y - i) + headright) * 4 + RedIndex] = 255;
                        stuff[(width * (y - i) + headright) * 4 + GreenIndex] = 0;
                        stuff[(width * (y - i) + headright) * 4 + BlueIndex] = 0;
                    }
                }
                int jointX, jointY;
                jointY = (headMax + headMin) / 2;
                jointX = (headleft + headright) / 2;
                head.X = jointX;
                head.Y = jointY;
                int off = 0;
                //if pixel is unknown distance, check its neighbor
                while (width * jointY + jointX + off < width && stuff[(width * jointY + jointX + off) * 4] < 80)
                {
                    off++;
                }
                head.Z = stuff[(width * jointY + jointX + off) * 4];//assign distance value
                if (head.Z < 75)
                {
                    head.Z = 1000;
                }
                
                //drawDot((int)head.X, (int)head.Y, stuff, 2);//note pass by reference is default
                // trackShoulders(new ThreadParameters((int)head.X, (int)head.Y),headMin-headMax, stuff);
                
            }
            if (head.Z < 75)
            {
                head.Z = 1000;
            }
            return stuff;
        }

        private void trackShoulders(ThreadParameters r,int offset,  byte[] stuff)
        {
            //find left shoulder
            ThreadParameters t = new ThreadParameters(r.x,r.y);
            t.y += (int)(offset/1.5);
            double slope = 0;
            while (slope < 1)
            {
                t.lastx = t.x;
                t.lasty = t.y;
                t.y+=5;
                //find the left edge
                if (t.y>0&&t.y<height&&t.x>0&&t.x<width)//check for out of bounds
                {
                    
                    while (stuff[(width * t.y + t.x - 1) * 4] > 90 && stuff[(width * t.y + t.x - 1) * 4] < 110)
                    {
                        t.x--;
                        if (stuff[(width * t.y + t.x - 1) * 4] == 33)//detect unknown distance as noise
                        {
                            int noise = 0;
                            int curx = t.x;
                            //attempt to skip past noise
                            while (!(stuff[(width * t.y + t.x - 1) * 4] > 90 && stuff[(width * t.y + t.x - 1) * 4] < 110))
                            {
                                noise++;
                                t.x--;
                                if (t.x == 1)
                                    break;
                            }
                            //Console.WriteLine(noise);
                            if (noise > 30)//if noise value is too big assume it's not noise and detect edge
                            {
                                t.x = curx;
                                break;
                            }
                        }
                    }
                    slope = 5.0 / (t.lastx - t.x);
                }
                else
                {
                    slope = -1;
                }
                //Console.WriteLine(slope);
            }

            drawDot(t.x, t.y, stuff, 1);

            //find right shoulder
            t = new ThreadParameters(r.x, r.y);
            t.y += (int)(offset / 1.5);
            slope = 0;
            while (slope < .60)
            {
                t.lastx = t.x;
                t.lasty = t.y;
                t.y += 5;
                //find the right edge
                if (t.y > 0 && t.y < height && t.x > 0 && t.x < width)//check for out of bounds
                {

                    while (stuff[(width * t.y + t.x + 1) * 4] > 90 && stuff[(width * t.y + t.x + 1) * 4] < 110)
                    {
                        t.x++;
                        if (stuff[(width * t.y + t.x + 1) * 4] == 33)//detect unknown distance as noise
                        {
                            int noise = 0;
                            int curx = t.x;
                            //attempt to skip past noise
                            while (!(stuff[(width * t.y + t.x + 1) * 4] > 90 && stuff[(width * t.y + t.x + 1) * 4] < 110))
                            {
                                noise++;
                                t.x++;
                                if (t.x == width-1)
                                    break;
                            }
                            //Console.WriteLine(noise);
                            if (noise > 30)//if noise value is too big assume it's not noise and detect edge
                            {
                                t.x = curx;
                                break;
                            }
                        }
                    }
                    slope = 5.0 / (t.x-t.lastx);
                }
                else
                {
                    slope = 1;
                }
                //Console.WriteLine(slope);
            }
            drawDot(t.x, t.y, stuff, 1);
        }

        //finds hand corresponding to input "right" if right = 1 then finds righthand, else it finds left.
        private void trackHand(byte[] stuff, int right)
        {
            int thresh = (int)head.Z + 2;
            //top left is 0,0 so handMin will be > hand Max
            int handMax = 0;
            int handMid = 0;
            int handleft = width;
            int handright = 0;
            int nochange = 0;
            int xinit = 0;
            int xend = 2;
            if (right == 1)
            {
                xinit = width / 2;
                xend = 1;
            }
            for (int y = 0; y < height; y++)
            {
                for (int x = xinit; x < width/xend; x++)
                {
                    //if processing in range data
                    if (stuff[(width * y + x) * 4] > thresh && stuff[(width * y + x) * 4] < 120)
                    {
                        if (handMax == 0)
                        {
                            //assume first in range item found is the top of the hand
                            handMax = y;
                            handleft = x;
                            handright = x;
                            break;
                        }
                    }
                }
                if (handMax > 0)
                {
                    //find left hand side
                    int x = handleft;
                    while (x > 0 && stuff[(width * y + x - 1) * 4] > thresh && stuff[(width * y + x - 1) * 4] < 120)
                    {
                        x--;
                        if (stuff[(width * y + x - 1) * 4] == 33)//detect unknown distance as noise
                        {
                            int noise = 0;
                            int curx = x;
                            //attempt to skip past noise
                            while (!(stuff[(width * y + x - 1) * 4] > thresh && stuff[(width * y + x - 1) * 4] < 120))
                            {
                                noise++;
                                x--;
                                if (x <= 0)
                                {
                                    break;
                                }
                            }
                            //Console.WriteLine(noise);
                            if (noise > 50 || x <= 0)//if noise value is too big assume it's not noise and detect edge
                            {
                                x = curx;
                            }
                        }
                    }
                        
                    //find right hand side
                    int xr = handright;
                    while (xr<width-1&&stuff[(width * y + xr + 1) * 4] > thresh && stuff[(width * y + xr + 1) * 4] < 120)
                    {
                        xr++;
                        if (stuff[(width * y + xr - 1) * 4] == 33)//detect unknown distance as noise
                        {
                            int noise = 0;
                            int curx = x;
                            //attempt to skip past noise
                            while (!(stuff[(width * y + xr + 1) * 4] > thresh && stuff[(width * y + xr + 1) * 4] < 120))
                            {
                                noise++;
                                xr++;
                                if (xr >= width-1)
                                {
                                    break;
                                }
                            }
                            //Console.WriteLine(noise);
                            if (noise > 50 || xr >= width-1)//if noise value is too big assume it's not noise and detect edge
                            {
                                xr = curx;
                                break;
                            }
                        }
                    }
                    //assign left and right values
                    if (handleft == x)
                    {
                        nochange++;
                    }
                    else
                    {
                        nochange = 0;
                        handMid = y;
                    }
                    handright = xr;
                    handleft = x;
                    
                }

                //terminate statement: if handleft does not change for 50 y values
                if (nochange > 50 && handMid-handMax>20||y+1==479)
                {
                    break;
                }
            }
            if (handMid == 0)
            {
                handMid = handMax;
            }
            if (handMid + (handMid - handMax)<height-1&&handMid>0)//assume if bottom of bounding box is lower than screen, hand is offscreen
            {
                SkeletonPoint hand = new SkeletonPoint();
                hand.Y = handMid;
                hand.X = (handright + handleft) / 2;
                int off = 0;
                while (width * hand.Y + hand.X + off < width && stuff[(int)((width * hand.Y + hand.X + off) * 4)] < 80)
                {
                    off++;
                }
                hand.Z = stuff[(int)((width * hand.Y + hand.X + off) * 4)];//assign distance value
                //if the calculated hand value is significantly close to the calculated head value do not save
                if (Math.Abs(hand.X - head.X) <= 10 || Math.Abs(hand.Y - head.Y) <= 10)
                {
                    return;
                }
                if (right == 1)
                {
                    //if the calculated righthand value is significantly close to the calculated lefthand value, do not save
                    if (Math.Abs(hand.X - leftHand.X) > 10 || Math.Abs(hand.Y - leftHand.Y) > 10)
                    {
                        drawDot((int)hand.X, (int)hand.Y, stuff, right);//note pass by reference is default
                        rightHand = hand;
                    }
                }
                else
                {
                    //if the calculated righthand value is significantly close to the calculated lefthand value, do not save
                    if (Math.Abs(hand.X - rightHand.X) > 10 || Math.Abs(hand.Y - rightHand.Y) > 10)
                    {
                        drawDot((int)hand.X, (int)hand.Y, stuff, right);//note pass by reference is default
                        leftHand = hand;
                    }
                }
                return;
            }
            //if hand is offscreen set z value to 1000;
            if (right == 1)
            {
                rightHand.Z = 1000;
            }
            else
            {
                leftHand.Z = 1000;
            }

        }

        //updates object position (called after the object has been found)
        private void updateObject(byte[] stuff, int obj)
        {
            //find top
            int y = 0;
            int x = 0;
            int thresh = 90;
            int top = 0;
            int min = 0;
            int nochange = 0;
            if (obj == 0)
            {
                y = (int)head.Y;
                x = (int)head.X;
            }
            else if (obj == 1)
            {
                y = (int)rightHand.Y;
                x = (int)rightHand.X;
                thresh = (int)head.Z + 3;
            }
            else
            {
                y = (int)leftHand.Y;
                x = (int)leftHand.X;
                thresh = (int)head.Z + 3;
            }
            if (x == -1 || y == -1)
            {
                return;
            }
            //identify left and right edges
            int xr = x;
            int left = x;
            int right = x;
            //find left side of object
            while (x > 0 && stuff[(width * y + x - 1) * 4] > thresh && stuff[(width * y + x - 1) * 4] < 120)
            {
                x--;
                if (stuff[(width * y + x - 1) * 4] == 33)//detect unknown distance as noise
                {
                    int noise = 0;
                    int curx = x;
                    //attempt to skip past noise
                    while (!(stuff[(width * y + x - 1) * 4] > thresh && stuff[(width * y + x - 1) * 4] < 120))
                    {
                        noise++;
                        x--;
                        if (x <= 0)
                        {
                            break;
                        }
                    }
                    //Console.WriteLine(noise);
                    if (noise > 50 || x <= 0)//if noise value is too big assume it's not noise and detect edge
                    {
                        x = curx;
                        break;
                    }
                }
            }
            left = x;

            //find right side of object
            while (xr < width - 1 && stuff[(width * y + xr + 1) * 4] > thresh && stuff[(width * y + xr + 1) * 4] < 120)
            {
                xr++;
                if (stuff[(width * y + xr - 1) * 4] == 33)//detect unknown distance as noise
                {
                    int noise = 0;
                    int curx = x;
                    //attempt to skip past noise
                    while (!(stuff[(width * y + xr + 1) * 4] > thresh && stuff[(width * y + xr + 1) * 4] < 120))
                    {
                        noise++;
                        xr++;
                        if (xr >= width - 1)
                        {
                            break;
                        }
                    }
                    //Console.WriteLine(noise);
                    if (noise > 50 || xr >= width - 1)//if noise value is too big assume it's not noise and detect edge
                    {
                        xr = curx;
                        break;
                    }
                }
            }
            right = xr;
            
            //scan up until the next line has no in-range value (findes absolute maximum)

            int xtemp = 0;
            for (int i = y; i > 0; i--)
            {
                bool complete = false;
                for (int j = left; j < right; j++)
                {
                    if (j == right - 1)
                    {
                        complete = true;
                        break;
                    }
                    if (stuff[(width * i + j) * 4] > thresh && stuff[(width * i + j) * 4] < 120)
                    {
                        top = i;
                        xtemp = j;
                        break;
                    }
                }
                if (complete)
                {
                    right = xtemp;
                    left = xtemp;
                    break;
                }
            }

            //run bounding algorithm
            for (int i = top; i < height; i++)
            {
                //find left side
                x = left;
                while (x > 0 && stuff[(width * y + x - 1) * 4] > thresh && stuff[(width * y + x - 1) * 4] < 120)
                {
                    x--;
                    if (stuff[(width * y + x - 1) * 4] == 33)//detect unknown distance as noise
                    {
                        int noise = 0;
                        int curx = x;
                        //attempt to skip past noise
                        while (!(stuff[(width * y + x - 1) * 4] > thresh && stuff[(width * y + x - 1) * 4] < 120))
                        {
                            noise++;
                            x--;
                            if (x <= 0)
                            {
                                break;
                            }
                        }
                        //Console.WriteLine(noise);
                        if (noise > 50 || x <= 0)//if noise value is too big assume it's not noise and detect edge
                        {
                            x = curx;
                        }
                    }
                }

                //find right side
                xr = right;
                while (xr < width - 1 && stuff[(width * y + xr + 1) * 4] > thresh && stuff[(width * y + xr + 1) * 4] < 120)
                {
                    xr++;
                    if (stuff[(width * y + xr - 1) * 4] == 33)//detect unknown distance as noise
                    {
                        int noise = 0;
                        int curx = x;
                        //attempt to skip past noise
                        while (!(stuff[(width * y + xr + 1) * 4] > thresh && stuff[(width * y + xr + 1) * 4] < 120))
                        {
                            noise++;
                            xr++;
                            if (xr >= width - 1)
                            {
                                break;
                            }
                        }
                        //Console.WriteLine(noise);
                        if (noise > 50 || xr >= width - 1)//if noise value is too big assume it's not noise and detect edge
                        {
                            xr = curx;
                            break;
                        }
                    }
                }
                //assign left and right values
                if (left == x)
                {
                    nochange++;
                }
                else
                {
                    nochange = 0;
                }
                right = xr;
                left = x;
                min = i;

                //terminate statement: if handleft does not change for 50 y values
                if (nochange > 50 && min - top > 30)
                {
                    break;
                }
            }
            if (min == 0)
            {
                min = top;
            }
            min = min + min - top;
            if (min < height - 1&& min>0)//assume if bottom of bounding box is lower than screen, hand is offscreen
            {
                //drawDot(width / 2, handMax, stuff, -1);
                //drawDot(width / 2, handMin, stuff, -1);
               /* drawDot(left, min, stuff, 2);
                drawDot(right, min, stuff, 2);
                drawDot((right + left) / 2, top, stuff, 1);
                drawDot((right + left) / 2, min, stuff, 0);*/
                //draw bounding box
                for (int i = 0; i < 2; i++)
                {
                    for (x = left; x <= right; x++)
                    {
                        y = min;
                        if (min >= height)
                        {
                            y = height - 1;
                        }
                        stuff[(width * (y) + x + i) * 4 + RedIndex] = 255;
                        stuff[(width * (y) + x + i) * 4 + GreenIndex] = 0;
                        stuff[(width * (y) + x + i) * 4 + BlueIndex] = (byte)(122 * obj);
                        stuff[(width * top + x + i) * 4 + RedIndex] = 255;
                        stuff[(width * top + x + i) * 4 + GreenIndex] = 0;
                        stuff[(width * top + x + i) * 4 + BlueIndex] = (byte)(122 * obj);
                    }
                    for (y = min; y >= top; y--)
                    {
                        int ny = y;
                        if (ny >= height - 1)
                        {
                            ny = height - 2;
                        }
                        stuff[(width * (ny + i) + left) * 4 + RedIndex] = 255;
                        stuff[(width * (ny + i) + left) * 4 + GreenIndex] = 0;
                        stuff[(width * (ny + i) + left) * 4 + BlueIndex] = (byte)(122 * obj);
                        stuff[(width * (ny + i) + right) * 4 + RedIndex] = 255;
                        stuff[(width * (ny + i) + right) * 4 + GreenIndex] = 0;
                        stuff[(width * (ny + i) + right) * 4 + BlueIndex] = (byte)(122 * obj);
                    }
                }
                SkeletonPoint part = new SkeletonPoint();
                part.Y = min-((min-top)/2);
                part.X = (right + left) / 2;
                int off = 0;
                while (width * part.Y + part.X + off < width && stuff[(int)((width * part.Y + part.X + off) * 4)] < 80)
                {
                    off++;
                }
                part.Z = stuff[(int)((width * part.Y + part.X + off) * 4)];//assign distance value
                //if values are incorrect re-initialize the object and return
                if (part.Z < 75||part.Z>120)
                {
                    part.X = 0;
                    part.Y = 0;
                    part.Z = 1000;
                    if (obj == 0)
                    {
                        head = part;
                    }
                    else if (obj == 1)
                    {
                        rightHand = part;
                    }
                    else
                    {
                        leftHand = part;
                    }
                    return;
                }
                if (obj == 0)
                {
                    if (Math.Abs(part.Y - head.Y) < 50 && Math.Abs(part.X - head.X) < 50)
                    {
                        head = part;
                    }

                    drawDot((int)part.X, (int)part.Y, stuff, obj);//note pass by reference is default
                }
                else if (obj == 1)
                {
                    if (Math.Abs(part.Y - rightHand.Y) < 50 && Math.Abs(part.X - rightHand.X) < 50)
                    {
                        rightHand = part;
                    }
                    drawDot((int)part.X, (int)part.Y, stuff, obj);//note pass by reference is default
                }
                else
                {
                    if (Math.Abs(part.Y - leftHand.Y) < 50 && Math.Abs(part.X - leftHand.X) < 50)
                    {
                        leftHand = part;
                    }
                    drawDot((int)part.X, (int)part.Y, stuff, obj);//note pass by reference is default
                }
                return;
            }
            //if hand is offscreen set z value to 1000;
            if (obj == 0)
            {
                head.Z = 1000;
                drawDot((int)head.X, (int)head.Y, stuff, obj);
            }
            else if (obj == 1)
            {
                rightHand.Z = 1000;
                drawDot((int)head.X, (int)head.Y, stuff, obj);
            }
            else
            {
                leftHand.Z = 1000;
                drawDot((int)head.X, (int)head.Y, stuff, obj);
            }


        }

        //draws a dot in any of the primary colors (includes black)
        private void drawDot(int x, int y, byte[] stuff, int offset)
        {
            for (int i = -5; i < 6; i++)
            {
                for (int j = -5; j < 6; j++)
                {
                    int index = (width * (y + i) + (x + j)) * 4;
                    if (index >= 0 && index < (width * height)*4)
                    {
                        stuff[index] = 0; //to set iterator to specific row it's rowWidth*row, then add offset so rowWidth*row+column
                        stuff[index + 1] = 0;
                        stuff[index + 2] = 0;
                        if (offset > -1)
                        {
                            stuff[index + offset] = 255;
                        }
                    }
                }
            }
        }

        private void captureButton_Click(object sender, RoutedEventArgs e)
        {
            if(sensor.ElevationAngle <27)
                sensor.ElevationAngle += 5;
        }

        private void downButton_Click(object sender, RoutedEventArgs e)
        {
            if(sensor.ElevationAngle >0)
                sensor.ElevationAngle -= 5;
        }
        
    }
}
